/*
 * @文件描述: DSP相关函数
 * @版本:
 * @作者: 周晨阳
 * @Date: 2021-01-02 15:51:14
 */

#include "arm_math.h"
#include "typedef.h"
#include "filter.h"

//int类型滤波器参数
#define BLOCK_SIZE 20 /* 调用一次arm_fir_f32处理的采样点个数 */
#define NUM_TAPS 40   /* 滤波器系数个数 必须为偶数*/
arm_fir_instance_q15 S;
uint16_t blockSize = BLOCK_SIZE;
static q15_t firState[BLOCK_SIZE + NUM_TAPS - 1]; /* 状态缓存，大小numTaps + blockSize - 1*/

//float类型滤波器参数
#define BLOCK_SIZE_FLOAT 20 /* 调用一次arm_fir_f32处理的采样点个数 */
#define NUM_TAPS_FLOAT 7    /* 滤波器系数个数 */
arm_fir_instance_f32 S_F;
uint32_t blockSizeFloat = BLOCK_SIZE_FLOAT;
static float32_t firStateF32[BLOCK_SIZE_FLOAT + NUM_TAPS_FLOAT - 1]; /* 状态缓存，大小numTaps + blockSize - 1*/

const q15_t firCoeffs[NUM_TAPS] = {
    37, 17, -21, -73, -114, -102, 0, 184, 368,
    417, 210, -265, -847, -1217, -1013, 0, 1779, 3958,
    5942, 7125, 7125, 5942, 3958, 1779, 0, -1013, -1217,
    -847, -265, 210, 417, 368, 184, 0, -102, -114,
    -73, -21, 17, 37};

const float32_t firCoeffs32LP[NUM_TAPS_FLOAT] = {
    0.01349692326, 0.07845086604, 0.2408624738, 0.3343794644,
    0.2408624738, 0.07845086604, 0.01349692326};
/**
 * @description: FIR数字低通滤波器，输入的数组长度必须为20的倍数，如 20，40，60
 * @param {q15_t} *input 输入数组
 * @param {q15_t} *output 输出数组
 * @param {int} blockSize 数组长度
 * @return {*}
 */
void firFilter(q15_t *input, q15_t *output, q15_t blockSize)
{
  int i;
  /* 实现FIR滤波 */
  for (i = 0; i < blockSize / BLOCK_SIZE; i++)
  {
    arm_fir_q15(&S, input + (i * blockSize), output + (i * blockSize),
                blockSize);
  }
}
/**
 * @description: FIR数字低通滤波器，输入的数组长度必须为20的倍数，如 20，40，60
 * @param {float} *input 输入数组
 * @param {float} *output 输出数组
 * @param {int} blockSize 数组长度
 * @return {*}
 */
void firFilterFloat(float *input, float *output, int blockSize)
{
  int i;
  /* 实现FIR滤波 */
  for (i = 0; i < blockSize / BLOCK_SIZE_FLOAT; i++)
  {
    arm_fir_f32(&S_F, input + (i * blockSize), output + (i * blockSize),
                blockSize);
  }
}
/**
 * @description: DSP模块初始化
 * @param {*}
 * @return {*}
 */
void DSP_Init()
{
  /* fir低通滤波器初始化 */
  arm_fir_init_f32(&S_F, NUM_TAPS_FLOAT, (float32_t *)&firCoeffs32LP[0],
                   &firStateF32[0], blockSizeFloat);

  if (arm_fir_init_q15(&S, NUM_TAPS, (q15_t *)&firCoeffs[0],
                       &firState[0], blockSize) == ARM_MATH_SUCCESS)
  {
    printf("DSP init done...\n");
  }
  else
  {
    printf("DSP init failed...\n");
  }
}
